#include <rclcpp/rclcpp.hpp>
#include <learning_interface/srv/get_point_cloud.hpp>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include <vector>
#include <thread>
#include <mutex>
#include <chrono>
#include <algorithm>
#include <unordered_set>

using namespace std::chrono_literals;

class TcpServerNode : public rclcpp::Node {
public:
  TcpServerNode() : Node("livox_client_node") {
    // 声明端口参数，默认8080
    this->declare_parameter<int>("port", 8888);
    port_ = this->get_parameter("port").as_int();

    // 创建服务客户端
    client_ = this->create_client<learning_interface::srv::GetPointCloud>("get_latest_pointcloud");

    // 启动TCP服务器线程
    server_thread_ = std::thread(&TcpServerNode::tcp_server, this);
    RCLCPP_INFO(this->get_logger(), "TCP服务器节点已启动，监听端口: %d", port_);
  }

  ~TcpServerNode() {
    running_ = false;
    
    // 关闭所有客户端套接字
    {
      std::lock_guard<std::mutex> lock(clients_mutex_);
      for (int sock : client_sockets_) {
        shutdown(sock, SHUT_RDWR);
        close(sock);
      }
      client_sockets_.clear();
    }

    if (server_thread_.joinable()) {
      server_thread_.join();
    }
  }

private:
  void tcp_server() {
    // 创建TCP套接字
    int server_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (server_fd < 0) {
      RCLCPP_ERROR(this->get_logger(), "创建套接字失败");
      return;
    }

    // 设置套接字选项
    int opt = 1;
    setsockopt(server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));

    // 绑定地址和端口
    sockaddr_in address;
    address.sin_family = AF_INET;
    address.sin_addr.s_addr = INADDR_ANY;
    address.sin_port = htons(port_);

    if (bind(server_fd, (sockaddr*)&address, sizeof(address)) < 0) {
      RCLCPP_ERROR(this->get_logger(), "绑定端口失败");
      close(server_fd);
      return;
    }

    // 监听连接
    if (listen(server_fd, 3) < 0) {
      RCLCPP_ERROR(this->get_logger(), "监听失败");
      close(server_fd);
      return;
    }

    running_ = true;
    while (running_) {
      // 接受客户端连接
      sockaddr_in client_addr;
      socklen_t client_len = sizeof(client_addr);
      int client_socket = accept(server_fd, (sockaddr*)&client_addr, &client_len);
      
      if (client_socket < 0) {
        if (running_) {
          RCLCPP_WARN(this->get_logger(), "接受连接失败");
        }
        continue;
      }

      // 记录新客户端
      {
        std::lock_guard<std::mutex> lock(clients_mutex_);
        client_sockets_.insert(client_socket);
      }

      // 处理客户端连接
      std::thread(&TcpServerNode::handle_client, this, client_socket).detach();
      
      char client_ip[INET_ADDRSTRLEN];
      inet_ntop(AF_INET, &(client_addr.sin_addr), client_ip, INET_ADDRSTRLEN);
      RCLCPP_INFO(this->get_logger(), "新的客户端连接: %s:%d", client_ip, ntohs(client_addr.sin_port));
    }

    close(server_fd);
  }

  void handle_client(int client_socket) {
    const std::vector<uint8_t> magic_command = {
      0x55, 0xAA, 0x55, 0xAA, 0xFF, 0xA3, 0x00, 0x00, 0x5B, 0x29, 0x16, 0xE9, 0x16, 0xE9
    };
    const size_t magic_size = magic_command.size();
    
    std::vector<uint8_t> recv_buffer;
    char temp_buffer[1024];
    bool client_active = true;
    
    while (running_ && client_active) {
      // 接收数据
      ssize_t bytes_read = recv(client_socket, temp_buffer, sizeof(temp_buffer), 0);
      
      if (bytes_read <= 0) {
        if (bytes_read == 0) {
          RCLCPP_INFO(this->get_logger(), "客户端断开连接");
        } else {
          RCLCPP_WARN(this->get_logger(), "接收数据错误");
        }
        client_active = false;
        break;
      }
      
      // 追加到接收缓冲区
      recv_buffer.insert(recv_buffer.end(), temp_buffer, temp_buffer + bytes_read);
      
      // 在缓冲区中搜索魔术指令
      bool found_magic = false;
      auto it = std::search(recv_buffer.begin(), recv_buffer.end(), 
                            magic_command.begin(), magic_command.end());
      
      while (it != recv_buffer.end()) {
        found_magic = true;
        RCLCPP_INFO(this->get_logger(), "收到有效指令，请求点云数据...");
        
        // 从缓冲区移除魔术指令
        recv_buffer.erase(recv_buffer.begin(), it + magic_size);
        
        // 请求点云服务
        auto request = std::make_shared<learning_interface::srv::GetPointCloud::Request>();
        auto future = client_->async_send_request(request);
        
        // 等待服务响应
        if (future.wait_for(3s) == std::future_status::ready) {
          auto response = future.get();
          RCLCPP_INFO(this->get_logger(), "收到点云数据，点数: %d", response->pointcloud.width);
          
          // 处理点云数据为100个点
          std::vector<uint8_t> point_data = process_pointcloud(response->pointcloud);
          
          // 发送点云数据
          if (send(client_socket, point_data.data(), point_data.size(), 0) < 0) {
            RCLCPP_ERROR(this->get_logger(), "发送点云数据失败");
            client_active = false;
            break;
          }
        } else {
          RCLCPP_WARN(this->get_logger(), "服务请求超时");
          // 发送空数据包表示超时
          std::vector<uint8_t> empty_data(100 * 16, 0);
          send(client_socket, empty_data.data(), empty_data.size(), 0);
        }
        
        // 继续在当前缓冲区中搜索魔术指令
        it = std::search(recv_buffer.begin(), recv_buffer.end(), 
                         magic_command.begin(), magic_command.end());
      }
      
      // 清理过大的缓冲区（保留最后可能的部分匹配）
      if (recv_buffer.size() > 1024) {
        size_t keep_size = std::min(magic_size - 1, recv_buffer.size());
        std::vector<uint8_t> new_buffer(
          recv_buffer.end() - keep_size, 
          recv_buffer.end()
        );
        recv_buffer.swap(new_buffer);
      }
    }
    
    // 清理客户端资源
    close(client_socket);
    {
      std::lock_guard<std::mutex> lock(clients_mutex_);
      client_sockets_.erase(client_socket);
    }
  }

  std::vector<uint8_t> process_pointcloud(const sensor_msgs::msg::PointCloud2& cloud) {
    // 目标点数
    const size_t target_points = 100;
    const size_t point_size = 16; // 每个点16字节 (4字节ID + 3*4字节坐标)
    std::vector<uint8_t> result(target_points * point_size, 0);

    // 检查点云是否为空
    if (cloud.width == 0) {
      RCLCPP_WARN(this->get_logger(), "收到空点云，返回全零数据");
      return result;
    }

    // 检查点云格式 (x,y,z)
    int x_offset = -1, y_offset = -1, z_offset = -1;
    
    for (const auto& field : cloud.fields) {
      if (field.name == "x") x_offset = field.offset;
      else if (field.name == "y") y_offset = field.offset;
      else if (field.name == "z") z_offset = field.offset;
    }

    if (x_offset < 0 || y_offset < 0 || z_offset < 0) {
      RCLCPP_ERROR(this->get_logger(), "点云格式不支持，缺少必要字段");
      return result;
    }

    // 采样点云数据
    const uint8_t* data_ptr = cloud.data.data();
    const size_t point_step = cloud.point_step;
    const size_t source_points = cloud.width * cloud.height;

    // 采样策略
    for (size_t i = 0; i < target_points; ++i) {
      size_t src_index;
      if (source_points <= target_points) {
        // 点云点数不足，循环使用现有点
        src_index = i % source_points;
      } else {
        // 点云点数过多，均匀采样
        src_index = (i * source_points) / target_points;
      }

      // 复制点数据 (ID + x,y,z)
      const uint8_t* src_ptr = data_ptr + src_index * point_step;
      uint8_t* dst_ptr = result.data() + i * point_size;
      
      // 写入点ID (4字节整数)
      int32_t id = static_cast<int32_t>(i);
      memcpy(dst_ptr, &id, 4);

      // 复制坐标数据
      memcpy(dst_ptr + 4, src_ptr + x_offset, 4);  // x
      memcpy(dst_ptr + 8, src_ptr + y_offset, 4);  // y
      memcpy(dst_ptr + 12, src_ptr + z_offset, 4); // z
    }

    return result;
  }

  // 成员变量
  rclcpp::Client<learning_interface::srv::GetPointCloud>::SharedPtr client_;
  std::thread server_thread_;
  std::mutex clients_mutex_;
  std::unordered_set<int> client_sockets_;
  int port_;
  bool running_ = false;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TcpServerNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
